
import matplotlib.pyplot as plt
import networkx as nx
import heapq
from Split_equally import split_path_equally
def draw_graph(G,start,end, path=None,title="搜寻的路径"):
    pos = {node: (node[0], node[1]) for node in G.nodes()}
    num_segments = 1
    # print(path)
    segmented_path = split_path_equally(path, num_segments)
    fig = plt.figure(figsize=(3,8))
    nx.draw(G, pos, node_size=0.2, node_color='lightblue', font_size=8, arrowstyle='-', arrowsize=5)
    plt.ion()
    if path:
        edges = [(path[i], path[i + 1]) for i in range(len(path) - 1)]
        # print(edges)
        nx.draw(G, pos,node_size=0.2, node_color='lightblue', font_size=8, arrowstyle='-', arrowsize=5, edgelist=edges, edge_color='r', width=2)
    plt.scatter(start[0], start[1], c='r')
    plt.scatter(end[0], end[1], c='r')
     # 启用交互模式
    a = []
    b = []
    for p in segmented_path:
        a.append(p[0])
        b.append(p[1])

        plt.scatter(a, b, s=5, color='green')
        plt.draw()
        plt.pause(0.3)

    plt.ioff()  # 退出交互模式
    plt.title(title)
    plt.show()




def dfs_path(G, start, end):
    # 初始化距离字典
    # distances = {node: float('inf') for node in G.nodes}
    # distances[start] = 0
    #
    # # 使用优先队列来存储节点和对应的最短距离
    # pq = [(0, start)]
    #
    # while pq:
    #     current_distance, current_node = heapq.heappop(pq)
    #
    #     # 如果当前节点已经处理过，就跳过
    #     if current_distance > distances[current_node]:
    #         continue
    #
    #     for neighbor, weight in G[current_node].items():
    #         distance = current_distance + weight
    #         # 如果找到了更短的路径，更新距离字典和优先队列
    #         if distance < distances[neighbor]:
    #             distances[neighbor] = distance
    #             heapq.heappush(pq, (distance, neighbor))
    #
    # # 从 end 开始回溯，得到最短路径
    # shortest_path = []
    # current = end
    # while current != start:
    #     shortest_path.append(current)
    #     for neighbor, weight in G[current].items():
    #         if distances[current] - weight == distances[neighbor]:
    #             current = neighbor
    #             break
    # shortest_path.append(start)
    # print(shortest_path)
    # return list(reversed(shortest_path))
    shortest_path = nx.shortest_path(G, source=start, target=end)
    # print(shortest_path)
    return shortest_path


def single_run(G,start_node,end_node):
    path = dfs_path(G, start_node, end_node)  # 执行深度优先搜索算法
    print(f'start_node:{start_node}\nend_node  :{end_node}')
    print(f'path      : {path}')
    if path:
        draw_graph(G, start_node,end_node, path,"路径规划",)
    else:
        print(f"从起点到终点没有找到路径")



if __name__ == '__main__':


    # print(G.nodes)
    # print(G)
    # with open("node.txt", "w") as file:
    #     file.write("")
    #
    # # 将G的节点写入node.txt文件中
    # with open("node.txt", "w") as file:
    #     for node in G.nodes():
    #         file.write(str(node) + "\n")
    from Map_maker import *
    start_node = (4.612602, -0.085868)  # 选择起点
    end_node =(18.169516, 28.700434) # 选择终点
    print(G.neighbors(start_node))
    single_run(G,start_node,end_node)